{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "    <div class=\"bk-root\">\n",
       "        <a href=\"https://bokeh.pydata.org\" target=\"_blank\" class=\"bk-logo bk-logo-small bk-logo-notebook\"></a>\n",
       "        <span id=\"1001\">Loading BokehJS ...</span>\n",
       "    </div>"
      ],
      "text/plain": [
       "\n",
       "    <div class=\"bk-root\">\n",
       "        <a href=\"https://bokeh.pydata.org\" target=\"_blank\" class=\"bk-logo bk-logo-small bk-logo-notebook\"></a>\n",
       "        <span id=\"1001\">Loading BokehJS ...</span>\n",
       "    </div>"
      ]
     },
     "execution_count": 0,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "text/html": [
       "\n",
       "    <div class=\"bk-root\">\n",
       "        <a href=\"https://bokeh.pydata.org\" target=\"_blank\" class=\"bk-logo bk-logo-small bk-logo-notebook\"></a>\n",
       "        <span id=\"1001\">Loading BokehJS ...</span>\n",
       "    </div>"
      ],
      "text/plain": [
       "\n",
       "    <div class=\"bk-root\">\n",
       "        <a href=\"https://bokeh.pydata.org\" target=\"_blank\" class=\"bk-logo bk-logo-small bk-logo-notebook\"></a>\n",
       "        <span id=\"1001\">Loading BokehJS ...</span>\n",
       "    </div>"
      ]
     },
     "execution_count": 0,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import bokeh.plotting as bk\n",
    "from bokeh.io import output_notebook\n",
    "output_notebook()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys\n",
    "sys.path.append('../') # go to parent dir\n",
    "import numpy as np\n",
    "np.set_printoptions(precision=3, formatter={'float': '{: 0.3f}'.format})\n",
    "np.seterr('warn')\n",
    "import os\n",
    "import cProfile, pstats, io"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "from bokeh.palettes import Category20\n",
    "colors = Category20[20]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Spline Data Generation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Create Config"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "steps = 100\n",
    "dt = 0.04\n",
    "\n",
    "config = {\n",
    "    'steps': steps + 1,\n",
    "    'd': 2,\n",
    "    'sd': 5,\n",
    "    'r': 0.05 ** 2 * np.identity(2),\n",
    "    'init_m': np.asarray([6.5, 2.5, 0.00, 12, 0.1]),\n",
    "    'p_basis': np.array([\n",
    "        [2.5, 0.0],\n",
    "        [2.5, 1.0],\n",
    "        [0.0, 1.0],\n",
    "        [-2.5, 1.0],\n",
    "        [-2.5, 0.0],\n",
    "        [-2.5, -1.0],\n",
    "        [0.0, -1.0],\n",
    "        [2.5, -1.0],\n",
    "    ]),\n",
    "    'mean_scat_number': 125,\n",
    "}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Run Simulator"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "ename": "ModuleNotFoundError",
     "evalue": "No module named 'data'",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-7-c843d38d2b6f>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[0;32m----> 1\u001b[0;31m \u001b[0;32mfrom\u001b[0m \u001b[0mdata\u001b[0m \u001b[0;32mimport\u001b[0m \u001b[0mSplineDataSimulator\u001b[0m \u001b[0;32mas\u001b[0m \u001b[0mSimulator\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m      2\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      3\u001b[0m \u001b[0mdata_source\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mSimulator\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdt\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mdt\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m**\u001b[0m\u001b[0mconfig\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      4\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      5\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mi\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mrange\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0msteps\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mModuleNotFoundError\u001b[0m: No module named 'data'"
     ],
     "output_type": "error"
    }
   ],
   "source": [
    "from data import SplineDataSimulator as Simulator\n",
    "\n",
    "data_source = Simulator(dt=dt, **config)\n",
    "\n",
    "for i in range(steps):\n",
    "    data_source.step()\n",
    "\n",
    "gt, measurements = data_source.extract()\n",
    "bboxes = data_source.extrackt_bbox()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'measurements' is not defined",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-8-430671a9c6ac>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      3\u001b[0m meas_source = ColumnDataSource(\n\u001b[1;32m      4\u001b[0m         data={\n\u001b[0;32m----> 5\u001b[0;31m             \u001b[0;34m'ts'\u001b[0m\u001b[0;34m:\u001b[0m \u001b[0mmeasurements\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'ts'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m      6\u001b[0m             \u001b[0;34m'x'\u001b[0m\u001b[0;34m:\u001b[0m \u001b[0mmeasurements\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'xy'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      7\u001b[0m             \u001b[0;34m'y'\u001b[0m\u001b[0;34m:\u001b[0m \u001b[0mmeasurements\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'xy'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mNameError\u001b[0m: name 'measurements' is not defined"
     ],
     "output_type": "error"
    }
   ],
   "source": [
    "from bokeh.models import ColumnDataSource\n",
    "\n",
    "meas_source = ColumnDataSource(\n",
    "        data={\n",
    "            'ts': measurements['ts'],\n",
    "            'x': measurements['xy'][:, 0],\n",
    "            'y': measurements['xy'][:, 1],\n",
    "        }\n",
    ")\n",
    "\n",
    "gt_bbox_source = ColumnDataSource(\n",
    "        data={\n",
    "            'ts': bboxes['ts'],\n",
    "            'm_x': bboxes['center_xy'][:, 0],\n",
    "            'm_y': bboxes['center_xy'][:, 1],\n",
    "            'phi': bboxes['orientation'][:],\n",
    "            'l': bboxes['dimension'][:, 0],\n",
    "            'w': bboxes['dimension'][:, 1],\n",
    "        }\n",
    ")\n",
    "\n",
    "gt_source = ColumnDataSource(\n",
    "        data={\n",
    "            'ts': gt['ts'],\n",
    "            'x': gt['m'][:, 0],\n",
    "            'y': gt['m'][:, 1],\n",
    "            'phi': gt['m'][:, 2],\n",
    "            'omega': gt['m'][:, 3],\n",
    "            'v': gt['m'][:, 3],\n",
    "        }\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": true
   },
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'meas_source' is not defined",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-9-e68d5a7b30f7>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      6\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      7\u001b[0m \u001b[0mtop\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mfigure\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mtools\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mTOOLS\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mwidth\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m800\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mheight\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m350\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtitle\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;32mNone\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mmatch_aspect\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;32mTrue\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0moutput_backend\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;34m\"webgl\"\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 8\u001b[0;31m \u001b[0mmeas_scatter\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mtop\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcircle\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'x'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m'y'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mcolor\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;34m'#303030'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mlegend\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;34m'measurements'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0msize\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0malpha\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m0.2\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0msource\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mmeas_source\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m      9\u001b[0m \u001b[0mtrack_plot\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mtop\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mline\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'x'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m'y'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mlegend\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;34m'track'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0msource\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mgt_source\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     10\u001b[0m hover = HoverTool(renderers=[track_plot],\n",
      "\u001b[0;31mNameError\u001b[0m: name 'meas_source' is not defined"
     ],
     "output_type": "error"
    }
   ],
   "source": [
    "from bokeh.layouts import column\n",
    "from bokeh.plotting import figure, output_file, show\n",
    "from bokeh.models import HoverTool, BoxSelectTool, PanTool, BoxZoomTool, WheelZoomTool, UndoTool, RedoTool, ResetTool, SaveTool\n",
    "\n",
    "TOOLS = [PanTool(), BoxSelectTool(), BoxZoomTool(), WheelZoomTool(), UndoTool(), RedoTool(), ResetTool(), SaveTool()]\n",
    "\n",
    "top = figure(tools=TOOLS, width=800, height=350, title=None, match_aspect=True, output_backend=\"webgl\")\n",
    "meas_scatter = top.circle('x', 'y', color='#303030', legend='measurements', size=1, alpha=0.2, source=meas_source)\n",
    "track_plot = top.line('x', 'y', legend='track', source=gt_source)\n",
    "hover = HoverTool(renderers=[track_plot],\n",
    "        tooltips=[\n",
    "            (\"index\", \"$index\"),\n",
    "            (r\"(x, y, phi, v)\", \"(@x, @y, @phi, @v)\"),\n",
    "        ]\n",
    "    )\n",
    "top.add_tools(hover)\n",
    "\n",
    "bottom = figure(tools=TOOLS, width=800, height=350, title=None, \n",
    "                x_range=top.x_range, y_range=top.y_range, output_backend=\"webgl\")\n",
    "bottom.circle('x', 'y', color='#303030', legend='measurements', size=1, alpha=0.2, source=meas_source)\n",
    "bottom.rect(x='m_x', y='m_y', width='l', height='w', angle='phi', color='#98df8a', \n",
    "            fill_alpha=0.2, source=gt_bbox_source, legend='ground truth boxes')\n",
    "\n",
    "bottom.legend.click_policy=\"hide\"\n",
    "\n",
    "p = column([top, bottom])\n",
    "show(p)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Save Data"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "path = os.path.join(os.getcwd(), 'data')\n",
    "if not os.path.exists(path):\n",
    "    os.makedirs(path)\n",
    "\n",
    "filename = r'simulated_data'\n",
    "np.save(os.path.join(path, filename + '.npy'), measurements)\n",
    "filename = r'gt_path'\n",
    "np.save(os.path.join(path, filename + '.npy'), gt)\n",
    "filename = r'gt_bboxes'\n",
    "np.save(os.path.join(path, filename + '.npy'), bboxes)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
